#include "cpurepursuit.h"

int main()
{
    CPurePursuit *ObjPurePursuit = new CPurePursuit();

    while(ObjPurePursuit->idxTarget < ObjPurePursuit->posRef[0].size()-1){
        //寻找预瞄距离范围内最近路径点
        look_ahead_t resAheadPoint = ObjPurePursuit->findLookAheadPoint(ObjPurePursuit->pos, ObjPurePursuit->v,
                                                        ObjPurePursuit->posRef, ObjPurePursuit->Kv, ObjPurePursuit->Ld0);

        ObjPurePursuit->idxTarget = resAheadPoint.idxTarget;

        //计算控制量
        double delta = ObjPurePursuit->purePursuit(resAheadPoint.lookAheadPoint, resAheadPoint.idxTarget,
                                   ObjPurePursuit->pos, ObjPurePursuit->heading,
                                   ObjPurePursuit->v, ObjPurePursuit->posRef,
                                   ObjPurePursuit->Kv, ObjPurePursuit->Ld0, ObjPurePursuit->L);

        //计算加速度
        double a = ObjPurePursuit->Kp * (ObjPurePursuit->targetSpeed - ObjPurePursuit->v)/ObjPurePursuit->dt;

        //更新状态量
        new_state_t newState = ObjPurePursuit->updataState(a, ObjPurePursuit->pos,
                                           ObjPurePursuit->heading, ObjPurePursuit->v, delta,
                                           ObjPurePursuit->L, ObjPurePursuit->dt);
        ObjPurePursuit->pos = newState.pos;
        ObjPurePursuit->heading = newState.heading;
        ObjPurePursuit->v = newState.v;

        //保存更新的状态量
        ObjPurePursuit->posActual.push_back(newState.pos);
        ObjPurePursuit->headingActual.push_back(newState.heading);
        ObjPurePursuit->vActual.push_back(newState.v);
    }

    for(int i = 0; i < ObjPurePursuit->posActual.size(); i++){
        for(int j = 0; j < ObjPurePursuit->posActual[0].size(); j++){
            if(j == 0)
                ObjPurePursuit->XActual.push_back(ObjPurePursuit->posActual[i][j]);
            else
                ObjPurePursuit->YActual.push_back(ObjPurePursuit->posActual[i][j]);
        }
    }

    // 画图
    //plt::plot(ObjPurePursuit->XRef, ObjPurePursuit->YRef, "r-",
    //          ObjPurePursuit->XActual, ObjPurePursuit->YActual, "b*");
    //plt::show();
    ObjPurePursuit->plotRes(ObjPurePursuit->XRef, ObjPurePursuit->YRef, ObjPurePursuit->XActual, ObjPurePursuit->YActual);

    delete ObjPurePursuit;
    return 0;
}
